#include <Arduino.h>
#include <Servo.h> 
#include <avr/io.h>
#include <avr/interrupt.h>
#include <math.h>
#include "config.h"
#include "motors.h"
#include "ADC.h"
#include "realese.h"
#include "Timer.h"
#include "setup.h"

void setup_realese(){  
  robotInit();
}

//VARIABLEAS
float front=0, right=0, left=0, b_left=0, b_right=0;

#define MINSPEED 80
#define MAXSPEED 120
#define OVERSPEED 180

#define DEG_TO_RAD 0.01745329238
#define RAD_TO_DEG 57.29578

double b_right_angle = 180 * DEG_TO_RAD;
double right_angle   = 135 * DEG_TO_RAD;
double front_angle   = 90 * DEG_TO_RAD;
double left_angle    = 45 * DEG_TO_RAD;
double b_left_angle  = 0 * DEG_TO_RAD;

double x_b_right, y_b_right,
x_right,   y_right,
x_front,   y_front,
x_left,    y_left,
x_b_left,  y_b_left;

double average_x;
double average_y;

double res_angle;
double res_speed;

void mRight ()
{
  writeServo(-30);
  stopp();
  delay(320);
  writeServo(30);
  delay(500);
  back(MINSPEED);
  delay(500);
  stopp();
  delay(220);
  writeServo(-30);
  delay(500);
  forward(MINSPEED);
  delay(250);
  /*
      rightTSOP=30;
   leftTSOP=30;
   centerTSOP=30;
   */

}

void mLeft ()
{
  writeServo(30);
  stopp();
  delay(320);
  writeServo(-30);
  delay(500);
  back(MINSPEED);
  delay(500);
  stopp();
  delay(220);
  writeServo(30);
  delay(500);
  forward(MINSPEED);
  delay(250);
}

int turL=0,turR=0;
void loop_realese()
{  
  while(1)
  { 
    // Получение показаний дальномеров
    right = read_gp2y0a_range(adc_data[DIST_SENSOR_RIGHT]);
    front = read_gp2y0a_range(adc_data[DIST_SENSOR_CENTER]);
    left = read_gp2y0a_range(adc_data[DIST_SENSOR_LEFT]);
    b_right = read_gp2y0a_range(adc_data[DIST_SENSOR_BRIGHT]);
    b_left = read_gp2y0a_range(adc_data[DIST_SENSOR_BLEFT]);

    if ( !digitalRead (BUTTON))
    {
      writeServo(0);
      delay(10);
      forward(OVERSPEED);
      delay(2000);
    }
    
    // ======= Система предотвращения столкновений ========
    if (!digitalRead(CONTACT_SENSOR_LEFT)) {stopp(); delay(300); mLeft();} 
    else if (!digitalRead(CONTACT_SENSOR_RIGHT)) {stopp(); delay(300); mRight();}
    
    if( !digitalRead(CONTACT_SENSOR_CENTER))
    {
      stopp();
      delay(50);
      if( !digitalRead(CONTACT_SENSOR_CENTER))
      {
        if (b_right > b_left)
          mLeft();
        else
          mRight();
        continue;
      }
    }
    
    if (front > 120)
    {
      right *= 2;
      left  *= 2;
      
      b_right = 50;
      b_left  = 50;
    }
    
    b_right *= 2;
    b_left *= 2;
    
    x_b_right = b_right * cos(b_right_angle);
    y_b_right = b_right * sin(b_right_angle);

    x_right = right * cos(right_angle);
    y_right = right * sin(right_angle);

    x_front = front * cos(front_angle);
    y_front = front * sin(front_angle);

    x_left = left * cos(left_angle);
    y_left = left * sin(left_angle);

    x_b_left = b_left * cos(b_left_angle);
    y_b_left = b_left * sin(b_left_angle);

    average_x = (x_b_right + x_right + x_front + x_left + x_b_left) / 5;
    average_y = (y_b_right + y_right + y_front + y_left + y_b_left) / 5;

    res_angle = atan(average_y/average_x) * RAD_TO_DEG;
    res_speed = sqrt(average_x*average_x + average_y*average_y);

    if (res_angle < 0)
      res_angle = 90 + res_angle;
    else
      res_angle = res_angle - 90;
    
    writeServo((int)res_angle);
    if((front<100)||(right<100)||(left<100))
    {  forward((int)90);    }
    else 
    {  forward((int)120);}

  }  
}










